home *** CD-ROM | disk | FTP | other *** search
/ 3D Games - Real-time Rend…ng & Software Technology / 3D Games - Real-time Rendering & Software Technology.iso / flysdk / lib / flyEngine / flyClasslib.cpp < prev    next >
Encoding:
C/C++ Source or Header  |  2000-12-20  |  7.1 KB  |  382 lines
  1. #include "../Fly3D.h"
  2.  
  3. void mat4x4::lerp(mat4x4& m1,mat4x4& m2,float t)
  4. {
  5.     quaternion q1(m1),q2(m2),qt;
  6.     qt.lerp(q1,q2,t);
  7.     qt.get_mat(*this);
  8.     m[3][0]=m1.m[3][0]*(1-t)+m2.m[3][0]*t;
  9.     m[3][1]=m1.m[3][1]*(1-t)+m2.m[3][1]*t;
  10.     m[3][2]=m1.m[3][2]*(1-t)+m2.m[3][2]*t;
  11. }
  12.  
  13. void mat4x4::rotate( float ang,vector& dir )
  14. {
  15.     static mat4x4 m;
  16.     m.set_rotation(ang,dir);
  17.     *this=*this*m;
  18. }
  19.  
  20. void mat4x4::set_rotation( float ang,vector& dir )
  21. {
  22.     dir.normalize();
  23.  
  24.     ang*=-PiOver180;
  25.  
  26.     float fCos=(float)cos( ang );
  27.     float fSin=(float)sin( ang );
  28.  
  29.     m[0][0] = ( dir.x * dir.x ) * ( 1.0f - fCos ) + fCos;
  30.     m[0][1] = ( dir.x * dir.y ) * ( 1.0f - fCos ) - (dir.z * fSin);
  31.     m[0][2] = ( dir.x * dir.z ) * ( 1.0f - fCos ) + (dir.y * fSin);
  32.  
  33.     m[1][0] = ( dir.y * dir.x ) * ( 1.0f - fCos ) + (dir.z * fSin);
  34.     m[1][1] = ( dir.y * dir.y ) * ( 1.0f - fCos ) + fCos ;
  35.     m[1][2] = ( dir.y * dir.z ) * ( 1.0f - fCos ) - (dir.x * fSin);
  36.  
  37.     m[2][0] = ( dir.z * dir.x ) * ( 1.0f - fCos ) - (dir.y * fSin);
  38.     m[2][1] = ( dir.z * dir.y ) * ( 1.0f - fCos ) + (dir.x * fSin);
  39.     m[2][2] = ( dir.z * dir.z ) * ( 1.0f - fCos ) + fCos;
  40.     
  41.     m[0][3] = m[1][3] = m[2][3] = 0;
  42.     m[3][0] = m[3][1] = m[3][2] = 0;
  43.     m[3][3] = 1;
  44. }
  45.  
  46. quaternion::quaternion(mat4x4 &mat)
  47. {
  48.  
  49.     float tr,s,q[4];
  50.     int i,j,k;
  51.  
  52.     int nxt[3] = {1, 2, 0};
  53.  
  54.     tr = mat.m[0][0] + mat.m[1][1] + mat.m[2][2];
  55.  
  56.     // check the diagonal
  57.     if (tr > 0.0) 
  58.     {
  59.         s = (float)sqrt(tr + 1.0f);
  60.         w = s/2.0f;
  61.         s = 0.5f/s;
  62.         x = (mat.m[1][2] - mat.m[2][1]) * s;
  63.         y = (mat.m[2][0] - mat.m[0][2]) * s;
  64.         z = (mat.m[0][1] - mat.m[1][0]) * s;
  65.     } 
  66.     else 
  67.     {        
  68.         // diagonal is negative
  69.         i = 0;
  70.         if (mat.m[1][1] > mat.m[0][0]) i = 1;
  71.         if (mat.m[2][2] > mat.m[i][i]) i = 2;
  72.         j = nxt[i];
  73.         k = nxt[j];
  74.  
  75.         s=(float)sqrt((mat.m[i][i]-(mat.m[j][j] + mat.m[k][k])) + 1.0);
  76.       
  77.         q[i]=s*0.5f;
  78.             
  79.         if(s!=0.0f) s = 0.5f/s;
  80.  
  81.         q[3] = (mat.m[j][k] - mat.m[k][j]) * s;
  82.         q[j] = (mat.m[i][j] + mat.m[j][i]) * s;
  83.         q[k] = (mat.m[i][k] + mat.m[k][i]) * s;
  84.  
  85.         x = q[0];
  86.         y = q[1];
  87.         z = q[2];
  88.         w = q[3];
  89.     }
  90. }
  91.  
  92. void quaternion::get_mat(mat4x4 &mat)
  93. {
  94.     float wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;
  95.  
  96.     // calculate coefficients
  97.     x2 = x + x; 
  98.     y2 = y + y; 
  99.     z2 = z + z;
  100.     xx = x * x2;
  101.     xy = x * y2;   
  102.     xz = x * z2;
  103.     yy = y * y2;   
  104.     yz = y * z2;   
  105.     zz = z * z2;
  106.     wx = w * x2;   
  107.     wy = w * y2;   
  108.     wz = w * z2;
  109.  
  110.     mat.m[0][0] = 1.0f-(yy+zz);     
  111.     mat.m[1][0] = xy - wz;
  112.     mat.m[2][0] = xz + wy;        
  113.     mat.m[3][0] = 0.0;
  114.  
  115.     mat.m[0][1] = xy + wz;        
  116.     mat.m[1][1] = 1.0f-(xx + zz);
  117.     mat.m[2][1] = yz - wx;        
  118.     mat.m[3][1] = 0.0;
  119.  
  120.     mat.m[0][2] = xz - wy;        
  121.     mat.m[1][2] = yz + wx;
  122.     mat.m[2][2] = 1.0f - (xx + yy);        
  123.     mat.m[3][2] = 0.0;
  124.  
  125.     mat.m[0][3] = 0;            
  126.     mat.m[1][3] = 0;
  127.     mat.m[2][3] = 0;            
  128.     mat.m[3][3] = 1;
  129. }
  130.  
  131. quaternion quaternion::operator *(quaternion &q)
  132. {
  133.     float A, B, C, D, E, F, G, H;
  134.     quaternion res;
  135.  
  136.     A = (q.w + q.x)*(w + x);
  137.     B = (q.z - q.y)*(y - z);
  138.     C = (q.w - q.x)*(y + z); 
  139.     D = (q.y + q.z)*(w - x);
  140.     E = (q.x + q.z)*(x + y);
  141.     F = (q.x - q.z)*(x - y);
  142.     G = (q.w + q.y)*(w - z);
  143.     H = (q.w - q.y)*(w + z);
  144.  
  145.     res.w = B + (-E - F + G + H) /2;
  146.     res.x = A - (E + F + G + H)/2; 
  147.     res.y = C + (E - F + G - H)/2; 
  148.     res.z = D + (E - F - G + H)/2;
  149.  
  150.     return res;
  151. }
  152.  
  153. void quaternion::normalize()
  154. {
  155.     float factor = 1.0f/(float)sqrt(x*x+y*y+z*z+w*w);
  156.  
  157.     x*=factor;
  158.     y*=factor;
  159.     z*=factor;
  160.     w*=factor;
  161. }
  162.  
  163. void quaternion::get_rotate(float &angle, vector &axis)
  164. {
  165.     angle=(float)acos(w)*2*PiUnder180;
  166.  
  167.     float f=(float)sin(angle*PiOver180*0.5f);
  168.  
  169.     axis.x=x/f;
  170.     axis.y=x/f;
  171.     axis.z=x/f;
  172. }
  173.  
  174. void quaternion::lerp(quaternion& q1,quaternion& q2,float t)
  175. {
  176.     float v;        // complement to t
  177.     float o;        // complement to v (t)
  178.     float theta;    // angle between q1 & q2
  179.     float sin_t;    // sin(theta)
  180.     float cos_t;    // cos(theta)
  181.     float phi;      // spins added to theta
  182.     int flip;       // flag for negating q2
  183.  
  184.     cos_t = q1[0] * q2[0] + q1[1] * q2[1] + q1[2] * q2[2] + q1[3] * q2[3];
  185.  
  186.     if (cos_t < 0.0)
  187.     {
  188.         cos_t = -cos_t;
  189.     flip = TRUE;
  190.     }
  191.     else flip = FALSE;
  192.  
  193.     if (1.0 - cos_t < 1e-6)
  194.     {
  195.         v = 1.0f - t;
  196.     }
  197.     else
  198.     {
  199.         theta = (float)acos(cos_t);
  200.         phi = theta;
  201.         sin_t = (float)sin(theta);
  202.         v = (float)sin(theta - t * phi) / sin_t;
  203.         o = (float)sin(t * phi) / sin_t;
  204.     }
  205.     if (flip) o = -o;
  206.  
  207.     x = v * q1[0] + o * q2[0];
  208.     y = v * q1[1] + o * q2[1];
  209.     z = v * q1[2] + o * q2[2];
  210.     w = v * q1[3] + o * q2[3];
  211. }
  212.  
  213. void local_system::align_z(vector& z)
  214. {
  215.     if (fabs(z.z)>=0.99f)
  216.         {
  217.         Y.x=-z.y*z.x;
  218.         Y.y=1-z.y*z.y;
  219.         Y.z=-z.y*z.z;
  220.  
  221.         Y.normalize();
  222.         X.cross(Y,z);
  223.         }
  224.     else 
  225.         {
  226.         Y.x=-z.z*z.x;
  227.         Y.y=-z.z*z.y;
  228.         Y.z=1-z.z*z.z;
  229.  
  230.         Y.normalize();
  231.         X.cross(Y,z);
  232.         }
  233.  
  234.     Z=z;
  235.  
  236.     update_mat();
  237. }
  238.  
  239. void local_system::update_mat()
  240. {
  241.     memset(&mat,0,sizeof(mat));
  242.  
  243.     mat.m[0][0]=X.x;
  244.     mat.m[0][1]=X.y;
  245.     mat.m[0][2]=X.z;
  246.  
  247.     mat.m[1][0]=Y.x;
  248.     mat.m[1][1]=Y.y;
  249.     mat.m[1][2]=Y.z;
  250.  
  251.     mat.m[2][0]=Z.x;
  252.     mat.m[2][1]=Z.y;
  253.     mat.m[2][2]=Z.z;
  254.     
  255.     mat.m[3][3]=1;
  256.  
  257.     int i,j;
  258.     for( i=0;i<4;i++ )
  259.         for( j=0;j<4;j++ )
  260.             mat_t.m[i][j]=mat.m[j][i];
  261.  
  262. }
  263.  
  264. void local_system::rotate(vector &v, vector &u, float maxang)
  265. {
  266.     vector c;
  267.     float f;
  268.     c.cross(v, u);
  269.     f=c.length();
  270.     if(f>0.01)
  271.     {
  272.         c.x/=f; c.y/=f; c.z/=f;
  273.         f=(float)acos(vec_dot(v, u))*180.0f/M_Pi;
  274.  
  275.         if(f>maxang)
  276.             f=maxang;
  277.         else if(f<-maxang)
  278.             f=-maxang;
  279.  
  280.         rotate(f, c);
  281.     }
  282. }
  283.  
  284. void local_system::rotate(vector& rot)
  285. {
  286.     static mat4x4 m;
  287.  
  288.     m.set_rotation(rot.x,X);
  289.     m.rotate(rot.y,Y);
  290.     m.rotate(rot.z,Z);
  291.  
  292.     X=X*m;
  293.     Y=Y*m;
  294.     Z=Z*m;
  295.     
  296.     update_mat();
  297. }
  298.  
  299. void local_system::rotate(float ang,vector& v)
  300. {
  301.     static mat4x4 m;
  302.  
  303.     m.set_rotation(ang,v);
  304.  
  305.     X=X*m;
  306.     Y=Y*m;
  307.     Z=Z*m;
  308.     
  309.     update_mat();
  310. }
  311.  
  312. void boundbox::add_point(vector& p)
  313. {
  314.     if (p.x>max.x)
  315.         max.x=p.x;
  316.     if (p.y>max.y)
  317.         max.y=p.y;
  318.     if (p.z>max.z)
  319.         max.z=p.z;
  320.  
  321.     if (p.x<min.x)
  322.         min.x=p.x;
  323.     if (p.y<min.y)
  324.         min.y=p.y;
  325.     if (p.z<min.z)
  326.         min.z=p.z;
  327. }
  328.  
  329. void boundbox::add_point(float x,float y,float z)
  330. {
  331.     if (x>max.x)
  332.         max.x=x;
  333.     if (y>max.y)
  334.         max.y=y;
  335.     if (z>max.z)
  336.         max.z=z;
  337.  
  338.     if (x<min.x)
  339.         min.x=x;
  340.     if (y<min.y)
  341.         min.y=y;
  342.     if (z<min.z)
  343.         min.z=z;
  344. }
  345.  
  346. int boundbox::ray_intersect(vector& ro,vector& rd,float& tnear,float& tfar)
  347. {
  348.     static float *rd_,*ro_,*min_,*max_,t1,t2,t;
  349.  
  350.     rd_=&rd.x;
  351.     ro_=&ro.x;
  352.     min_=&min.x;
  353.     max_=&max.x;
  354.     tnear=-BIG;
  355.     tfar=BIG;
  356.  
  357.     int a;
  358.     for( a=0;a<3;a++ )
  359.      {
  360.       if (rd_[a]>-SMALL && rd_[a]<SMALL)
  361.          if (ro_[a]<min_[a] || ro_[a]>max_[a])
  362.             return 0;
  363.          else ;
  364.       else {
  365.             t1=(min_[a]-ro_[a])/rd_[a];
  366.             t2=(max_[a]-ro_[a])/rd_[a];
  367.             if (t1>t2)
  368.                { t=t1; t1=t2; t2=t; }
  369.             if (t1>tnear)
  370.                tnear=t1;
  371.             if (t2<tfar)
  372.                tfar=t2;
  373.             if (tnear>tfar || tfar<SMALL)
  374.                return 0;
  375.            }
  376.      }
  377.     if (tnear>tfar || tfar<SMALL)
  378.        return 0;
  379.     return 2;
  380. }
  381.  
  382.